www.gusucode.com > Sphero Connectivity Package 程序工具箱matlab源码 > Sphero Connectivity Package/examples/sphero_motion_control.m

    %% Sphero Motion Control
% This example shows how to control the motion of a Sphero using the 
% Sphero Connectivity Package
%%
% <matlab:edit('sphero_motion_control') Open this example>
%% Introduction
% In this example, we will use the data received from the onboard sensors
% of the Sphero to perform closed loop control of the motion of the Sphero.
% We will provide a set of points that we want the Sphero to pass through. 
% The data from the sensors will be used to estimate the current position 
% of the Sphero, which will be used by the controller to compute the speed 
% and angle with which the Sphero will be commanded to move.
%
%% Prerequisites
% It is helpful to complete the <matlab:showdemo('sphero_getting_started') 
% Getting Started with Sphero Connectivity Package> example.
%
%% Create a Sphero object (if it does not exist)

if ~exist('sph','var'),
    sph = sphero(); % Create a Sphero object
end

% make sure the object is connected
connect(sph);

% ping it
result = ping(sph);

% interrupt the example if ping was not successful
if ~result
    disp('Example aborted due to unsuccessful ping');
    return, 
end

%%
% The Bluetooth name of the device that you would like to connect to can be 
% specified when using the *sphero* command, instead of searching for all
% the paired devices.
%%
% Turn on handshaking in order to make sure that the Sphero is able to 
% receive and respond to messages. Also, calibrate the orientation of the
% Sphero so that it points in the desired direction, before we start moving
% it around.
sph.Handshake = 1;  %Turn on handshaking 
sph.BackLEDBrightness = 255;
calibrate(sph, 0); %Calibrate the orientation of the sphero. Use this 
% command with different values of the angle in order to orient the Sphero in the desired direction.

%% Specify the points to be traversed
% Specification of the x and y coordinates of the points to be traversed on
% the plane (in cm):
despoints = [0 100;
            -50 100;
            50 75;
            -50 25;
            50 0;
            0 0];
 
numpoints = size(despoints, 1);

%Read the current location of the Sphero
[xstart, ystart, ~, ~, groundspeed] = readLocator(sph);

% Plot the points to be traversed
labels = cellstr(num2str([1:numpoints]') );  % labels correspond to the order in which the points are to be traversed
figure(1)
clf
plot(despoints(:, 1), despoints(:,2), 'b+')
text(despoints(:,1), despoints(:,2), labels, 'VerticalAlignment','bottom', ...
                             'HorizontalAlignment','right')
title('Points to be traversed');
axis([-100 100 -50 150])
hold on
plot(double(xstart), double(ystart), 'ko');
hold off

%%
% Specify the other parameters that are used in controlling the Sphero:

tfinal = 30; % Time limit on the motion of the Sphero
stopRadius = 3;  % Radius of the circle around the point, within which the Sphero should try to stop 
maxspeed = 150; % Max speed for saturation 
minspeed = -150;  % Min speed for saturation
restartspeed = 50; % Minimum speed required to restart the Sphero, if it 
% stops at a point where it is not supposed to stop. This minumum speed is
% required to get the Sphero to start moving again, due to its inertia

% Controller gains
Kp = 1; 
Ki = 0.1; 
Kd = 0.1;

% Initialize the variables to store the x, y coordinates of the points that
% the Sphero actually goes through, and the distance from the desired point 
xlog = []; 
ylog = []; 
distlog = [];

%% Closed loop control of Sphero to traverse specified points
% Initialize the variables for traversing the points:
idx = 1;
xcur = double(xstart);
ycur = double(ystart);
t0 = cputime;

t = [];
%%
% Run the while loop until the timout occurs, or when all points have been
% traversed. The *control_sphero* function implements the PID
% Controller, which outputs the desired speed of the robot, based on the
% distance between the current point and the next point that has to be
% reached.

while(cputime-t0<tfinal) && idx<=numpoints
    xdes = despoints(idx, 1);
    ydes = despoints(idx, 2);
    
    % Angle and distance calculation
    % Angle by which the Sphero should be rotated and the distance that it 
    % should move by in order to reach desired position. 
    % The angle is measured with respect to the Sphero's y-axis 
    % (or orientation of sphero)
    angle = rad2deg(atan2(double(xdes-xcur), double(ydes-ycur)));
    dist = sqrt((xdes-double(xcur)).^2 + (ydes-double(ycur)).^2); %Distance or the error   
       
    %Clear the persistent variables in the function, from the previous run.
    %If these variables are not cleared, the error values from the previous
    %run will be used, which can cause issues
    control_sphero(dist, double(groundspeed), Kp, Ki, Kd, stopRadius, maxspeed, minspeed, restartspeed, 1);
    
    while dist>stopRadius
        speed = control_sphero(dist, double(groundspeed), Kp, Ki, Kd, stopRadius, maxspeed, minspeed, restartspeed, 0);

        % Move the robot in the desired direction (specified by the 'angle'
        % with regards to the y-orientation of the sphero)
        result = roll(sph, speed, angle);

        % Read the current position and speed of the robot
        [xcur, ycur,~, ~, groundspeed] = readLocator(sph);
        
        % Angle and distance calculation
        angle = rad2deg(atan2(double(xdes-xcur), double(ydes-ycur)));
        dist = sqrt((xdes-double(xcur)).^2 + (ydes-double(ycur)).^2); %Distance or the error

        xlog(end+1) = xcur;
        ylog(end+1) = ycur;
        distlog(end+1) = dist;
        t(end+1) = cputime-t0;
    end
    %Increment the index to the next point that has to be traversed
    idx = idx+1;
end

brake(sph);

%% Plot the result
hold on
plot(xlog, ylog, 'rx');
hold off
legend('Desired points for traversal', 'Starting location', 'Motion of Sphero')

figure(2)
plot(distlog)
title('Error between the position of Sphero and desired point')

%%
% 
% <<sphero_motion_control_fig1.png>>
%
% <<sphero_motion_control_fig2.png>>
%
%% Disconnect the Sphero
disconnect(sph);
%Clear the persistent variables in the control function
control_sphero(dist, groundspeed, Kp, Ki, Kd, stopRadius, maxspeed, minspeed, restartspeed, 1);

%% Summary
% This example introduced an application example where the motion of the
% Sphero is controlled based on the onboard readings, in order to trasverse
% certain points on the plane.
%% More About
% <matlab:doc('control_sphero') Controller for Sphero motion>
%% See Also
% <matlab:showdemo('sphero_examples') Sphero Connectivity Package Examples>

%%
% Copyright 2015, The MathWorks, Inc.